/******************************************************************
Copyright © LibKz 2017~2023. All rights reserved.
* @projectName   LibKz
* @author        Raiiwhen
* @email         masterraii@icloud.com
* @origin        \05_Console\01_WatchX\WatchX\ALGO
*******************************************************************/
#ifndef _KALAMNFILTER_H_
#define _KALAMNFILTER_H_
typedef struct{
	float X[2];
	float Q[2];
	float P[2][2];
	float R;
	float K[2];	
	float dt;
}Filter_K;

void Kalmen_Filte_Init(	Filter_K* obj,
													float P00, float P01,
													float P10, float P11,
													float Q0, float Q1,
													float R,
													float dt
												);
float Kalmen_Fusion(Filter_K* obj, float gyro, float angle);
#ifndef _ATTITUDE_H_
#define _ATTITUDE_H_

#define KALMEN_EN

typedef struct{
	float x;
	float y;
	float z;
}vector3;

void IMU_ALGO_Init(void);
void IMU_Update(void);
float Kalmen_Filter(float gyro, float acc);

float get_accx(void);
float get_accy(void);
float get_accz(void);
float get_velx(void);
float get_vely(void);
float get_velz(void);
float get_posx(void);
float get_posy(void);
float get_posz(void);

float get_gaccx(void);
float get_gaccy(void);
float get_gaccz(void);
float get_gyrox(void);
float get_gyroy(void);
float get_gyroz(void);
float get_anglx(void);
float get_angly(void);
float get_anglz(void);

void rst_anglx(void);
void rst_angly(void);
void rst_anglz(void);

float get_pitch(void);
float get_roll(void);
float get_yaw(void);

float get_IMUTem(void);

void set_posx(float x0);
void set_posy(float y0);
void set_posz(float z0);

#endif
